#include "task_modbus.h"
#include "Modbus.h"
#include "btla_protocol.h"
#include "tlmbms_protocol.h"
#include "upper_com_protocol.h"
#include <string.h>
#include <stdio.h>
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
#include "main.h"
#include "usbd_cdc_if.h"

static modbusHandler_t ModbusH_1;
static modbusHandler_t ModbusH_2;

/* -----------------End of PCS functions----------------- */
static void modbus1ServiceRunner(void *args);
static void Modbus1TaskRunner(void *args);
static void modbus2ServiceRunner(void *args);
static void Modbus2TaskRunner(void *args);
static void modbusMonitorRunner(void *args);
static void taskPowerOn(void *args);

/* Definitions for modbus1 Service */
osThreadId_t modbus1ServiceHandle;
const osThreadAttr_t modbus1Service_attributes = {
    .name = "modbus1Service",
    .stack_size = 128 * 4,
    .priority = (osPriority_t)osPriorityLow,
};

/* Definitions for modbus2 Service */
osThreadId_t modbus2ServiceHandle;
const osThreadAttr_t modbus2Service_attributes = {
    .name = "modbus2Service",
    .stack_size = 128 * 4,
    .priority = (osPriority_t)osPriorityLow,
};

/* Definitions for ModbusTask1 */
osThreadId_t ModbusTask1Handle;
const osThreadAttr_t ModbusTask1_attributes = {
    .name = "ModbusTask1",
    .stack_size = 512 * 4,
    .priority = (osPriority_t)osPriorityLow,
};

/* Definitions for ModbusTask2 */
osThreadId_t ModbusTask2Handle;
const osThreadAttr_t ModbusTask2_attributes = {
    .name = "ModbusTask2",
    .stack_size = 512 * 4,
    .priority = (osPriority_t)osPriorityLow,
};

/* Definitions for PowerOnTask */
osThreadId_t PowerOnTaskHandle;
const osThreadAttr_t PowerOnTask_attributes = {
    .name = "PowerOnTask",
    .stack_size = 128 * 4,
    .priority = (osPriority_t)osPriorityLow,
};

modbusHandler_t *get_modbusH_1()
{
    return &ModbusH_1;
}

modbusHandler_t *get_modbusH_2()
{
    return &ModbusH_2;
}

void ModbusRegister()
{
    /* Master 注册 */
    ModbusH_1.uModbusType = MB_MASTER;
    ModbusH_1.u8id = 0; // For master it must be 0
    ModbusH_1.u16timeOut = 1000;
    ModbusReg(&ModbusH_1);

    ModbusH_2.uModbusType = MB_MASTER;
    ModbusH_2.u8id = 0;
    ModbusH_2.u16timeOut = 1000;
    ModbusReg(&ModbusH_2);
}

void Modbus1TimerInit()
{
    LL_USART_InitTypeDef USART_InitStruct = {0};
    float bytes_frame = 1; // 包含一个起始位
    float bytes_stop = 0;
    uint16_t us_stop = 0;

    /* PCS 串口参数以及定时器参数调整 */
    LL_USART_Disable(MODBUS_1_USART);
    LL_USART_DeInit(MODBUS_1_USART);
    USART_InitStruct.BaudRate = 38400;
    USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B;
    USART_InitStruct.StopBits = LL_USART_STOPBITS_1;
    USART_InitStruct.Parity = LL_USART_PARITY_NONE;
    USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX;
    USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE;
    USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16;
    LL_USART_Init(MODBUS_1_USART, &USART_InitStruct);
    LL_USART_ConfigAsyncMode(MODBUS_1_USART);
    LL_USART_Enable(MODBUS_1_USART);
    // 计算3.5个字符需要延迟的时间
    switch (USART_InitStruct.DataWidth)
    {
    case LL_USART_DATAWIDTH_8B:
        bytes_frame += 8;
        break;
    case LL_USART_DATAWIDTH_9B:
        bytes_frame += 9;
        break;
    default:
        bytes_frame += 8;
        break;
    }
    if (USART_InitStruct.Parity != LL_USART_PARITY_NONE)
        bytes_frame += 1;
    switch (USART_InitStruct.StopBits)
    {
    case LL_USART_STOPBITS_0_5:
        bytes_frame += 1.5;
        break;
    case LL_USART_STOPBITS_1:
        bytes_frame += 1;
        break;
    case LL_USART_STOPBITS_2:
        bytes_frame += 2;
        break;
    case LL_USART_STOPBITS_1_5:
        bytes_frame += 1.5;
        break;
    default:
        bytes_frame += 1;
        break;
    }
    bytes_stop = T35_M1 * bytes_frame; // 
    us_stop = bytes_stop * 1000. / USART_InitStruct.BaudRate * 100.;
    LL_TIM_SetAutoReload(TIM2, us_stop-1);
    LL_TIM_SetPrescaler(TIM2, 840 - 1); // 10us
    LL_USART_EnableIT_RXNE(MODBUS_1_USART);
}

void Modbus2TimerInit()
{
    LL_USART_InitTypeDef USART_InitStruct = {0};
    float bytes_frame = 1; // 包含一个起始位
    float bytes_stop = 0;
    uint16_t us_stop = 0;
    /* BMS 串口参数以及定时器参数调整 */
    LL_USART_Disable(MODBUS_2_USART);
    LL_USART_DeInit(MODBUS_2_USART);
    USART_InitStruct.BaudRate = 9600;
    USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B;
    USART_InitStruct.StopBits = LL_USART_STOPBITS_1;
    USART_InitStruct.Parity = LL_USART_PARITY_NONE;
    USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX;
    USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE;
    USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16;
    LL_USART_Init(MODBUS_2_USART, &USART_InitStruct);
    LL_USART_ConfigAsyncMode(MODBUS_2_USART);
    LL_USART_Enable(MODBUS_2_USART);
    // 计算3.5个字符需要延迟的时间
    bytes_frame = 1; // 包含一个起始位
    switch (USART_InitStruct.DataWidth)
    {
    case LL_USART_DATAWIDTH_8B:
        bytes_frame += 8;
        break;
    case LL_USART_DATAWIDTH_9B:
        bytes_frame += 9;
        break;
    default:
        bytes_frame += 8;
        break;
    }
    if (USART_InitStruct.Parity != LL_USART_PARITY_NONE)
        bytes_frame += 1;
    switch (USART_InitStruct.StopBits)
    {
    case LL_USART_STOPBITS_0_5:
        bytes_frame += 1.5;
        break;
    case LL_USART_STOPBITS_1:
        bytes_frame += 1;
        break;
    case LL_USART_STOPBITS_2:
        bytes_frame += 2;
        break;
    case LL_USART_STOPBITS_1_5:
        bytes_frame += 1.5;
        break;
    default:
        bytes_frame += 1;
        break;
    }
    bytes_stop = T35_M2 * bytes_frame; // 
    us_stop = bytes_stop * 1000. / USART_InitStruct.BaudRate * 100.;
    LL_TIM_SetAutoReload(TIM3, us_stop-1);
    LL_TIM_SetPrescaler(TIM3, 840 - 1); // 10us
    LL_USART_EnableIT_RXNE(MODBUS_2_USART);
}

void taskModbusInit()
{
    ModbusRegister();
    Modbus1TimerInit();
    Modbus2TimerInit();
    modbus1ServiceHandle = osThreadNew(modbus1ServiceRunner, NULL, &modbus1Service_attributes);
    modbus2ServiceHandle = osThreadNew(modbus2ServiceRunner, NULL, &modbus2Service_attributes);
    ModbusTask1Handle = osThreadNew(Modbus1TaskRunner, NULL, &ModbusTask1_attributes);
    ModbusTask2Handle = osThreadNew(Modbus2TaskRunner, NULL, &ModbusTask2_attributes);
    // PowerOnTaskHandle = osThreadNew(taskPowerOn, NULL, &PowerOnTask_attributes);

    // svcTaskAdd(1, 1, modbus1ServiceRunner, 0);    //1ms
    // svcTaskAdd(1000, 1, Modbus1TaskRunner, 0);   //1s
    // svcTaskAdd(10000, 1, modbusMonitorRunner, 0);   //10s
    // svcTaskAdd(5000,0,taskPowerOn,0);
}

void modbus1ServiceRunner(void *args)
{
    for (;;)
    {
        uint32_t tick=osKernelGetTickCount();
        TaskModbusMaster(&ModbusH_1);
        osDelayUntil(tick+1);//精确延时一毫秒
    }
}

void modbus2ServiceRunner(void *args)
{
    for (;;)
    {
        uint32_t tick=osKernelGetTickCount();
        TaskModbusMaster(&ModbusH_2);
        osDelayUntil(tick+1);//精确延时一毫秒
    }
}

void Modbus1TaskRunner(void *args)
{
    for (;;)
    {
        uint32_t tick=osKernelGetTickCount();
        {
            LL_GPIO_ResetOutputPin(LED1_GPIO_Port,LED1_Pin); //LED1亮起
            while(btla_all_regs_read(&ModbusH_1)!=12)   // 从从机读取寄存器,一共12个,返回n=0-11表示已经读到第n个了，返回12表示读取结束
                UNUSED(NULL);
            #if 0
            {
                dev_status_t *dev_status = get_status();
                usb_printf("\r\n");
                usb_printf("设备状态:");
                if (dev_status->short_resting)
                    usb_printf("短静置 ");
                if (dev_status->shutdown)
                    usb_printf("停机 ");
                if (dev_status->fault)
                    usb_printf("故障 ");
                if (dev_status->soft_start)
                    usb_printf("软起 ");
                if (dev_status->constant_voltage_op)
                    usb_printf("恒压运行 ");
                if (dev_status->constant_current_op)
                    usb_printf("恒流运行 ");
                if (dev_status->standby)
                    usb_printf("待机 ");
                if (dev_status->off_grid_op)
                    usb_printf("离网逆变运行 ");
                if (dev_status->AC_constant_power_op)
                    usb_printf("AC恒功率运行 ");
                usb_printf("\r\n");

                usb_printf("告警代码:%02X\r\n", *get_alarm_code());
                usb_printf("故障代码:%02X\r\n", *get_fault_code());
                usb_printf("直流电压:%.3fV\r\n", (float)get_DC_0_001V() / 1000.);
                usb_printf("直流电流:%.3fA\r\n", (float)get_DC_0_001A() / 1000.);
                usb_printf("直流功率:%dW\r\n", get_DC_W());
                usb_printf("当前累计:%.3fWh\r\n", (float)get_in_mode_0_001Wh() / 1000.);
                usb_printf("充电累计:%.3fWh\r\n", (float)get_charge_0_001Wh() / 1000.);
                usb_printf("放电累计:%.3fWh\r\n", (float)get_discharge_0_001Wh() / 1000.);
                usb_printf("电池状态:%02X\r\n", *get_batt_status());
                usb_printf("轻度告警:%02X\r\n", *get_batt_minor_alarm());
                usb_printf("中度告警:%02X\r\n", *get_batt_moderate_alarm());
                usb_printf("重度告警:%02X\r\n", *get_batt_severe_alarm());
                usb_printf("最大允许充电电流:%.1fA\r\n", (float)*get_batt_max_charge_0_1A() / 10.);
                usb_printf("最大允许放电电流:%.1fA\r\n", (float)*get_batt_max_discharge_0_1A() / 10.);
                usb_printf("最大允许充电功率:%uW\r\n", *get_batt_max_charge_100W() * 100);
                usb_printf("最大允许放电功率:%uW\r\n", *get_batt_max_discharge_100W() * 100);
                usb_printf("BMS电压:%.1fV\r\n", (float)*get_BMS_0_1V() / 10.);
                usb_printf("BMS电流:%.1fA\r\n", (float)*get_BMS_0_1A() / 10.);
                usb_printf("BMSSOC:%.2f%%\r\n", (float)*get_BMS_SOC_0_01pct() / 100.);
                usb_printf("BMSSOH:%.2f%%\r\n", (float)*get_BMS_SOH_0_01pct() / 100.);
                usb_printf("电网电压:%.3fV\r\n", (float)get_grid_0_001V() / 1000.);
                usb_printf("电网电流:%.3fA\r\n", (float)get_grid_0_001A() / 1000.);
                usb_printf("系统有功:%dW\r\n", get_sys_W());
                usb_printf("系统无功:%dVar\r\n", get_sys_Var());
                usb_printf("视在功率:%uVA\r\n", get_sys_VA());
                usb_printf("功率因数:%.3f\r\n", (float)*get_0_001PF() / 1000.);
                usb_printf("频率:%.2f\r\n", (float)*get_0_01Hz() / 100.);
                usb_printf("母线电压:%.1fV\r\n", (float)*get_bus_0_1V() / 10.);
                usb_printf("运行时间:%u:%u:%u.%u\r\n", *get_op_hours(), *get_op_minutes(), *get_op_seconds(), *get_op_milliseconds());

                usb_printf("直流最大电压:%.3fV\r\n", (float)get_DC_max_0_001V() / 1000.);
                usb_printf("直流最小电压:%.3fV\r\n", (float)get_DC_min_0_001V() / 1000.);
                usb_printf("最大充电电流:%.3fA\r\n", (float)get_DC_max_charge_0_001A() / 1000.);
                usb_printf("最大放电电流:%.3fA\r\n", (float)get_DC_max_discharge_0_001A() / 1000.);
                usb_printf("最大充电功率:%.3fW\r\n", (float)get_DC_max_charge_0_001W() / 1000.);
                usb_printf("最大放电功率:%.3fW\r\n", (float)get_DC_max_discharge_0_001W() / 1000.);

                usb_printf("交流电压上限:%.3fV\r\n", (float)get_AC_max_0_001V() / 1000.);
                usb_printf("交流电压下限:%.3fV\r\n", (float)get_AC_min_0_001V() / 1000.);
                usb_printf("交流电流上限:%.3fA\r\n", (float)get_AC_max_0_001A() / 1000.);
                usb_printf("交流频率上限:%.3fHz\r\n", (float)*get_AC_freq_max_0_01Hz() / 100.);
                usb_printf("交流频率下限:%.3fHz\r\n", (float)*get_AC_freq_min_0_01Hz() / 100.);

                usb_printf("霍尔变比:%u\r\n", *get_hall_ratio());
            }
            #endif
            LL_GPIO_SetOutputPin(LED1_GPIO_Port,LED1_Pin);//LED1熄灭
        }
        osDelayUntil(tick+1*osKernelGetTickFreq());//精确延迟一秒，考虑程序运行时间
    }
}

void Modbus2TaskRunner(void *args)
{
    for (;;)
    {
        uint32_t tick=osKernelGetTickCount();
        {
            LL_GPIO_ResetOutputPin(LED2_GPIO_Port,LED2_Pin); //LED2亮起
            while(tlmbms_RO_regs_read(&ModbusH_2)!=9)    // 从从机读取寄存器
                UNUSED(NULL);
            // uint8_t state_code ;
            // do
            // {
            //     state_code = tlmbms_RO_regs_read(&ModbusH_2);
            //     // usb_printf("%d\r\n",state_code);
            // } while (state_code!=9);
            upper_com_transmit();
            #if 0
            {
                usb_printf("SOC:%d%%\r\n", *get_bms_SOC());
                usb_printf("总电压:%.1fV\r\n", *get_bms_total_0_1V_1()/10.);
                usb_printf("总电流:%.1fA\r\n", *get_bms_total_0_1A_1()/10.);
            }
            #endif
            LL_GPIO_SetOutputPin(LED2_GPIO_Port,LED2_Pin);//LED2熄灭
        }
        osDelayUntil(tick+1*osKernelGetTickFreq());//精确延迟一秒，考虑程序运行时间
    }
}

void taskPowerOn(void *args)
{
    osDelay(osKernelGetTickFreq());
    write_start_stop_CMD(&ModbusH_1, true);
    for(;;){
        osDelay(osKernelGetTickFreq());
        static dev_operation_mode_t mode;
        mode.op_mode = 0x22;
        mode.para1 = -50000; //负为放电
        mode.para2 = 0;
        mode.para3 = 0;
        mode.para4 = 0;
        usb_printf("-50\r\n");
        write_operation_mode(&ModbusH_1, &mode);
        // vTaskDelete(PowerOnTaskHandle);



        osDelay(osKernelGetTickFreq()*3);
        mode.op_mode = 0x22;
        mode.para1 = 50000; //正为充电
        mode.para2 = 0;
        mode.para3 = 0;
        mode.para4 = 0;
        usb_printf("50\r\n");
        write_operation_mode(&ModbusH_1, &mode);
        // write_start_stop_CMD(&ModbusH_1, false);
        // vTaskDelete(PowerOnTaskHandle);
    }
}

void modbusMonitorRunner(void *args)
{
    for (;;)
    {
        TaskModbusMonitor(NULL);
    }
}


void TaskModbusMonitor(void *argument){
	#ifdef MONITOR_PRINT
	usb_printf("Modbus:\r\n");
    extern modbusHandler_t *mHandlers[MAX_M_HANDLERS];
    extern uint8_t numberHandlers;
	if(numberHandlers==0){
		usb_printf("No handler has been registered! \r\n");
		return;
	}
	for(int i=0;i<numberHandlers;i++){
		switch (mHandlers[i]->uModbusType)
		{
		case MB_SLAVE:
			usb_printf("Slave ");
			break;
		case MB_MASTER:
			usb_printf("Master ");
			break;
		default:
			usb_printf("Unknow Type ");
			break;
		}
		usb_printf("ID:0x%02X\r\n",mHandlers[i]->u8id);
		if(mHandlers[i]->u16errCnt!=0){
			usb_printf(" Last Error Code: %d\r\n",mHandlers[i]->i8lastError);
		}
		usb_printf(" state code: %d\r\n",mHandlers[i]->i8state);
		usb_printf(" ERR count: %d\r\n",mHandlers[i]->u16errCnt);
		usb_printf(" IN  count: %d\r\n",mHandlers[i]->u16InCnt);
		usb_printf(" OUT count: %d\r\n",mHandlers[i]->u16OutCnt);
	}
	#endif
}